/*
 * controller.cpp
 *
 *  Created on: Dec 11, 2012
 *      Author: michiel
 */
#include "navigation/controller.h"

void Controller::markerCallback(const ar_pose::ARMarkers::ConstPtr &msg) {
	int timediff = ros::Time::now().sec - time_.sec;
	navigation::motion motionMsg;

	motionMsg.state = BREAK;

	if (msg->markers.empty() && timediff > 0) {
		ROS_INFO("No markers detected!");
		ROS_INFO("Time difference: %i", timediff);

	} else if (msg->markers.empty() && timediff < 1) {
		motionMsg.state = ACCEL;
	} else {
		motionMsg.state = ACCEL;
		xpos_ = msg->markers[0].pose.pose.position.x * -10;
		zpos_ = msg->markers[0].pose.pose.position.z;
		time_ = msg->markers[0].header.stamp;
	}

	if (zpos_ > 0.50) {
		motionMsg.speed = zpos_;
	} else {
		motionMsg.speed = 0;
	}
	motionMsg.turn = xpos_;
	engine_pub_.publish(motionMsg);
}

void Controller::init() {
	marker_sub_ = nh_.subscribe(Topic::AR_POSE, 1, &Controller::markerCallback, this);
	engine_pub_ = nh_.advertise<navigation::motion>(Topic::NAV_VELOCITY, 1);
}

void Controller::spin() {
  ROS_INFO("Spinning");
  ros::spin();
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "main_controller");

	Controller c;
	c.spin();
	return 0;
}
